﻿using UnityEngine;
using System.Collections;
using System.Collections.Generic;


public sealed class CubicBezierSpline : Spline {




	/// <summary>
	/// Gets the point "t" in curve world coordinates.
	/// </summary>
	/// <returns>The point in curve in world coordinates.</returns>
	/// <param name="t">T.</param>
	public Vector3 GetPointInCurveWorldCoordinates(float t){
		return transform.TransformPoint(GetPointInCurve(t));
	}


	/// <summary>
	/// Gets the point in curve.
	/// </summary>
	/// <returns>The point in curve in local coordinates.</returns>
	/// <param name="t">T.</param>
	public Vector3 GetPointInCurve(float t){
		int currentCurve = GetCurveSectionNumber(t);
		float reminder = t - (percentagePerSection * (currentCurve - 1));
		float newT =  reminder / percentagePerSection;
		if(currentCurve > curveCount){
			currentCurve = curveCount;
			newT = 1f;
		}
		return GetPointInCurve(newT, currentCurve - 1);
	}

	/// <summary>
	/// Gets the curve section number at "t", considering the whole curve.
	/// </summary>
	/// <returns>The curve number.</returns>
	/// <param name="t">T.</param>
	public int GetCurveSectionNumber(float t){
		int count = 0;
		float percentages = 0;
		while(percentages <= t){
			percentages += percentagePerSection;
			count++;
		}
		return count;
	}

	/// <summary>
	/// Gets the point "t" in a specified curve section.
	/// </summary>
	/// <returns>The point in curve.</returns>
	/// <param name="t">T.</param>
	/// <param name="section">Section.</param>
	public Vector3 GetPointInCurve(float t, int section){
		int firstControlIndex = section * 3;
		return GetPointInCurve(t, controlPoints[firstControlIndex], controlPoints[firstControlIndex+1], controlPoints[firstControlIndex+2], controlPoints[firstControlIndex+3]);
	}

	/// <summary>
	/// Gets the point t in curve determined by 4 vectors.
	/// </summary>
	/// <returns>The point in curve.</returns>
	/// <param name="t">T.</param>
	/// <param name="point0">Point0; starting point</param>
	/// <param name="point1">Point1; control point 1</param>
	/// <param name="point2">Point2; control point 2</param>
	/// <param name="point3">Point3; endpoint</param>
	public override Vector3 GetPointInCurve(float t,
	                             Vector3 point0, Vector3 point1, Vector3 point2, Vector3 point3)
	{
		float u = 1 - t;
		float tSquared = t*t;
		float uSquared = u*u;
		float uCubed = uSquared * u;
		float tCubed = tSquared * t;
		
		Vector3 p = uCubed * point0; 	//first term
		p += 3 * uSquared * t * point1; //second term
		p += 3 * u * tSquared * point2; //third term
		p += tCubed * point3; 			//fourth term
		
		return p;
	}




	//TODO: Refactor in a better way
	public Vector3 PerpendicularOfPoint(float t){
		float threshold = 0.05f;
		if(t > 0 + threshold && t < 1f - threshold){
			Vector3 prevPoint = GetPointInCurve(t - threshold);
			Vector3 nextPoint = GetPointInCurve(t + threshold);

			Vector3 newVect = prevPoint - nextPoint;

			return new Vector3(-newVect.y, newVect.x, newVect.z);
		}
		else if(t < threshold){
			Vector3 point = GetPointInCurve(t);
			Vector3 nextPoint = GetPointInCurve(t + threshold);

			Vector3 newVect = point - nextPoint;

			return new Vector3(-newVect.y, newVect.x, newVect.z);

		}
		else {
			Vector3 prevPoint = GetPointInCurve(t - threshold);
			Vector3 point = GetPointInCurve(t);

			Vector3 newVect = prevPoint - point;

			return new Vector3(-newVect.y, newVect.x, newVect.z);
		}
	}
		

	public Vector3 GetControlPoint(int index){
		return (index * 3 < controlPoints.Count ? controlPoints[3 * index] : Vector3.zero);
	}

	public List<Vector3> ControlPoints {
		get { return controlPoints; }
	}

	public int ControlPointCount {
		get { return controlPoints.Count; }
	}

	public Vector3 GetHeadControllPoint() {
		return (controlPoints.Count > 0) ? controlPoints [0] : Vector3.zero;
	}

	public Vector3 GetTailControlPoint() {
		return (controlPoints.Count > 0) ? controlPoints [controlPoints.Count - 1] : Vector3.zero;
	}
}
